#include <iostream>
#include <cmath> // For math functions
#include <algorithm> 
#include "utm_transformer.h"
bool use_numpy = true;
const double K0 = 0.9996; 

const double E = 0.00669438;
const double E2 = E * E;
const double E3 = E2 * E;
const double E_P2 = E / (1.0 - E);

const double SQRT_E = std::sqrt(1 - E);
const double _E = (1 - SQRT_E) / (1 + SQRT_E);
const double _E2 = _E * _E;
const double _E3 = _E2 * _E;
const double _E4 = _E3 * _E;
const double _E5 = _E4 * _E;

const double M1 = (1 - E / 4 - 3 * E2 / 64 - 5 * E3 / 256);
const double M2 = (3 * E / 8 + 3 * E2 / 32 + 45 * E3 / 1024);
const double M3 = (15 * E2 / 256 + 45 * E3 / 1024);
const double M4 = (35 * E3 / 3072);

const double P2 = (3. / 2 * _E - 27. / 32 * _E3 + 269. / 512 * _E5);
const double P3 = (21. / 16 * _E2 - 55. / 32 * _E4);
const double P4 = (151. / 96 * _E3 - 417. / 128 * _E5);
const double P5 = (1097. / 512 * _E4);

const double R = 6378137;

const std::string ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX";

bool in_bounds(double x, double lower, double upper, bool upper_strict = false) {
    if (upper_strict && use_numpy) {
        return lower <= x && x < upper;
    } else if (upper_strict && !use_numpy) {
        return lower <= x && x < upper;
    } else if (use_numpy) {
        return lower <= x && x <= upper;
    }
    return lower <= x && x <= upper;
}
void check_valid_zone(int zone_number, char zone_letter) {
    if (!(1 <= zone_number && zone_number <= 60)) {
        std::cout << "zone number out of range (must be between 1 and 60)" << std::endl;
    }

    if (std::isalpha(zone_letter)) {
        zone_letter = std::toupper(zone_letter);

        if (!(zone_letter >= 'C' && zone_letter <= 'X') || zone_letter == 'I' || zone_letter == 'O') {
            std::cout << "zone letter out of range (must be between C and X)" << std::endl;
        }
    }
}


bool negative(double x) {
    

    if (use_numpy) {
        return x < 0;
    } else {
        return x < 0;
    }
}
double mod_angle(double value) {
    double pi = 3.14159265358979323846; // Approximate value of pi

    return std::fmod((value + pi), (2 * pi)) - pi;
}


char latitude_to_zone_letter(double latitude) {
    
    if (-80 <= latitude && latitude <= 84) {

        return ZONE_LETTERS[static_cast<int>(latitude + 80) >> 3];
    } else {
        return '\0';
    }
    


}
int latlon_to_zone_number(double latitude, double longitude) {
    
    if (56 <= latitude && latitude < 64 && 3 <= longitude && longitude < 12) {
        return 32;
    }

    if (72 <= latitude && latitude <= 84 && longitude >= 0) {
        if (longitude < 9) {
            return 31;
        } else if (longitude < 21) {
            return 33;
        } else if (longitude < 33) {
            return 35;
        } else if (longitude < 42) {
            return 37;
        }
    }

    return static_cast<int>((longitude + 180) / 6) + 1;
}
double zone_number_to_central_longitude(int zone_number) {
    return (zone_number - 1) * 6.0 - 180.0 + 3.0;
}
std::pair<double, double> from_latlon(double latitude, double longitude, int force_zone_number, char force_zone_letter ) {
    
    bool value_;
    value_=in_bounds(latitude,-80.0,84.0);
    if (!value_) {
        std::cout << "latitude out of range (must be between 80 deg S and 84 deg N)" << std::endl;
    }
    value_=in_bounds(latitude,-180.0,180.0);
    if (!value_) {
        std::cout << "longitude out of range (must be between 180 deg W and 180 deg E)" << std::endl;
    }
    if (force_zone_number != -1) {
        check_valid_zone(force_zone_number,force_zone_letter);
    }

    //double lat_rad = from_degrees(latitude);
    double lat_rad = latitude * M_PI / 180.0; 
    double lat_sin = std::sin(lat_rad);
    double lat_cos = std::cos(lat_rad);
    double lat_tan = lat_sin / lat_cos;
    double lat_tan2 = lat_tan * lat_tan;
    double lat_tan4 = lat_tan2 * lat_tan2;

    int zone_number;
    char zone_letter;

    if (force_zone_number == -1) {
        zone_number = latlon_to_zone_number(latitude, longitude);
    } else {
        zone_number = force_zone_number;
    }

    if (force_zone_letter == '\0') {
        zone_letter = latitude_to_zone_letter(latitude);
    } else {
        zone_letter = force_zone_letter;
    }

    //double lon_rad = from_degrees(longitude);
    double lon_rad = longitude * M_PI / 180.0; 
    double central_lon = zone_number_to_central_longitude(zone_number);
    //double central_lon_rad = from_degrees(central_lon);
    double central_lon_rad = central_lon * M_PI / 180.0; 

    double n = R / std::sqrt(1 - E * lat_sin * lat_sin);
    double c = E_P2 * lat_cos * lat_cos;

    double a = lat_cos * mod_angle(lon_rad - central_lon_rad);
    double a2 = a * a;
    double a3 = a2 * a;
    double a4 = a3 * a;
    double a5 = a4 * a;
    double a6 = a5 * a;

    double m = R * (M1 * lat_rad - M2 * std::sin(2 * lat_rad) +
                    M3 * std::sin(4 * lat_rad) - M4 * std::sin(6 * lat_rad));

    double easting = K0 * n * (a +
                               a3 / 6 * (1 - lat_tan2 + c) +
                               a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2)) + 500000;

    double northing = K0 * (m + n * lat_tan * (a2 / 2 +
                                               a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c * c) +
                                               a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - 330 * E_P2)));

    
    if (negative(latitude)) {
        northing += 10000000;
    }

    return std::make_pair(easting, northing); // use pair to package data,But you can only pack two pieces of data
}
/*
int main() {
    double lan = 30.0;  // Example latitude value
    double lon = 104.0; 
    std::pair<double, double> coordinates= from_latlon(lan,lon);
    double x = coordinates.first;
    double y = coordinates.second;
    
    std::cout << "Easting: " << x << std::endl;
    std::cout << "Northing: " << y << std::endl;

    
    
    return 0;
}*/









